跳到主要内容

迁移使用 Gazebo Classic 的 ROS 2 软件包 — Gazebo Harmonic 文档

迁移使用 Gazebo Classic 的 ROS 2

Gazebo 模拟器源于 Gazebo Classic 项目,但两者之间存在一些显著差异,这些差异会影响 ROS 2 项目使用该模拟器的方式。其中一个区别是,ROS 2 项目现在使用 ros_gz 包(而非 gazebo_ros_pkgs) 作为启动文件和其他实用工具的来源。另一个主要区别是, gazebo_ros_pkgs 提供了一组插件,这些插件可由 Gazebo Classic 直接加载并作为模拟的一部分运行,从而提供 ROS 和 Gazebo Classic 之间的接口,而 ros_gz 主要用作 ROS 和 gz-transport 主题之间的桥梁。了解这些概念上的差异对于过渡至关重要。

注意: 由于项目名称经历了两次重大更改,我们强烈建议您阅读项目 历史记录 ,以便更好地理解本教程和其他地方使用的术语。按照惯例,我们将旧版本的 Gazebo(发布号为 Gazebo 9 和 Gazebo 11 的版本)称为“Gazebo Classic”。较新版本的 Gazebo(以前称为“Ignition”),以字母开头的发布名称(如 Harmonic),在本文档中简称为“Gazebo”。本教程将展示如何将使用 包的现有 ROS 2 包迁移 gazebo_ros_pkgs 到新的 ros_gz 。我们将使用 turtlebot3_simulations 包作为示例。本教程中介绍的 的完整迁移版本 可在 此 fork turtlebot3_simulations 中找到 。

我们将首先按照 PC 设置 指南安装模拟 Turtlebot3 所需的先决条件。这将安装其他软件包,例如 Nav2Cartographer ,我们将在本教程的后面部分使用它们,因此请确保不要跳过此步骤。

下一步是克隆 turtlebot3_simulation 包。我们将使用 humble-devel 分支,在撰写本文时,它的 SHA 为 d16cdbe

source /opt/ros/humble/setup.bash
mkdir -p ~/turtlebot3_ws/src
cd ~/turtlebot3_ws/src
git clone -b humble-devel https://github.com/ROBOTIS-GIT/turtlebot3_simulations/

使用安装依赖项 rosdep

sudo rosdep init # only needed if  using rosdep
rosdep install --from-paths . -i -y

最后,构建项目并检查 Gazebo 经典模拟是否正常运行。(请参阅 Gazebo 模拟指南

cd ~/turtlebot3_ws
colcon build --symlink-install
source ~/turtlebot3_ws/install/setup.bash

export TURTLEBOT3_MODEL=waffle
ros2 launch turtlebot3_gazebo empty_world.launch.py

下面是启动获得的Turtlebot3在Gazebo Classic中运行的截图 empty_world.launch.py

Turtlebot 3 在 Gazebo Classic 中运行的屏幕截图

一旦我们确定 Gazebo 经典模拟正常运行,我们就会创建一个新的分支,在其中进行更改以迁移到新的 Gazebo。

git checkout -b new_gazebo

我们需要做出的改变是:

  1. 修改 package.xmlCMakeLists.txt 文件,用来自的包替换 gazebogazebo_ros_pkgsros_gz
  2. 编辑启动 Gazebo 的启动文件(例如 empty_world.launch.py
  3. 更新世界 SDFormat 文件。
  4. 编辑生成模型的启动文件。
  5. 编辑模型 SDFormat 文件。
  6. 桥接 ROS 主题。

更新软件包依赖

turtlebot 3 软件包依赖于 gazebo_ros_pkgs ,它提供启动文件、插件和其他实用程序,用于将 Gazebo classic 与 ROS 2 结合使用。新版 Gazebo 的对应软件包是 ros_gz ,但 ros_gz 它实际上是一个包含多个软件包的元软件包。 gazebo_ros_pkgsros_gz 这里可以用 替换 ,但仅使用项目所需的软件包子集可以减少依赖项的数量。对于 软件包, 目前 turtlebot3_simulation 我们只需要 ros_gz_bridgeros_gz_image 和。 和 提供 Gazebo 和 ROS 之间的主题桥梁,而 提供启动文件和其他实用程序,以帮助启动 Gazebo 和生成模型。 ros_gz_sim ros_gz_bridge ros_gz_image ros_gz_sim

进行更改后,第 17-21 行将 package.xml 如下所示:

您可以在此处找到 Gazebo Classic 包 XML 文件 ,并 在此处找到更新的 Gazebo 包 XML 文件

完成更改后,我们需要安装新的依赖项。以下命令将自动安装必要的 Gazebo 版本。

rosdep install --from-paths . -i -y

本教程假设您使用的是 Gazebo Fortress,因为它是与 ROS 2 Humble 正式配对的 Gazebo 版本。虽然可以将较新版本的 Gazebo 与 ROS 2 Humble 配合使用,但这需要额外的操作,因此不建议大多数用户使用。请参阅 使用 ROS 安装 Gazebo 了解更多信息。如果您打算在新版 Gazebo 和 Gazebo Classic 之间切换,最好使用 Gazebo Fortress,因为较新版本会自动卸载 Gazebo Classic。话虽如此,本教程涵盖的概念应该适用于较新版本的 ROS 2 和 Gazebo。

启动世界

现在我们需要编辑 turtlebot3_gazebo/launch/empty_world.launch.py 并替换所有 gazebo_ros_pkgs 。您可以 在此处找到编辑前的 Gazebo Classic 空世界启动文件 ,并 在此处找到更新后的 Gazebo 空世界启动文件 。首先将 的调用替换为 get_package_share_directory find ros_gz_sim 。代码将从:

到:

接下来,更改 gzserver_cmd 为使用 ros_gz_sim

gzserver_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(ros_gz_sim, 'launch', 'gz_sim.launch.py')
),
launch_arguments={'gz_args': ['-r -s -v4 ', world], 'on_exit_shutdown': 'true'}.items()
)

这将使用软件包 gz_sim.launch.py 中的启动文件 ros_gz_sim 。启动文件接受一个 gz_args 参数,该参数是将传递给 ( 在 Garden 及更高版本中)的命令行标志列表。 这将仅运行 Gazebo 服务器,而无需 GUI 客户端,并 指示 Gazebo 立即开始运行模拟。最后,我们使用一个 标志来设置 Gazebo 控制台输出的详细程度。 ign gazebo gz sim -s -r -v4

注意: 赋值给 的列表 gz_args 会被连接成一个字符串,其代码相当于 ''.join(gz_args) ,因此务必在必要时保留空格。请注意 中 4 后面的空格 。 '-v4 '

在运行 Gazebo 之前,参数 world 将被替换为 launch 。在此启动文件中, world 是一个 Python 变量,因此可以使用 Python 字符串格式: 。但如果我们想使用 变量来代替 ,则需要使用列表,以便 进行替换。 gz_args: f'-s -r -v4 {world}' LaunchConfiguration world launch

on_exit_shutdown 参数确保如果 Gazebo 服务器因任何原因退出,则启动文件中的其余节点将关闭

GUI 客户端以类似的方式启动,但我们改为 gz_args-g 运行 GUI 客户端。

gzclient_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(ros_gz_sim, 'launch', 'gz_sim.launch.py')
),
launch_arguments={'gz_args': '-g -v4 '}.items()
)

最后,我们需要设置环境变量 GZ_SIM_RESOURCE_PATH ,以便 Gazebo 知道在哪里查找模型。请参阅 查找资源 文档,了解有关此环境变量的更多信息。对于 ,不需要这样做, gazebo_ros_pkgs 因为它使用 <export> 中的标签 package.xml 填充了 Gazebo 的类似环境变量 ( GAZEBO_MODEL_PATH)。

首先,导入 AppendEnvironmentVariable

from launch.actions import AppendEnvironmentVariable

并创建一个 launch 操作,将目录的位置附加到环境变量 modelsturtlebot3_gazebo

然后我们需要将操作添加到 由 返回的变量 ld 中 。 LaunchDescription generate_launch_description

ld.add_action(set_env_vars_resources)

现在我们可以测试启动文件了。注释掉 ld.add_action(spawn_turtlebot_cmd) 并运行:

ros2 launch turtlebot3_gazebo empty_world.launch.py

这很可能会失败, 因为 Gazebo 无法找到世界 SDFormat 文件中引用的模型。下一步是修复这个问题。

注意: 由于 GUI 客户端存在 bug, 终止启动后可能会出现延迟 或进程。您可以使用以下命令终止它 ign gazebo -g gz sim -g pkill -f -9 'ign gazebo'

编辑世界 SDFormat 文件

我们将要编辑的文件是 turtlebot3_gazebo/worlds/empty_world.world 。该文件引用了模型 sunground_plane 。在 Gazebo Classic 中,这些模型要么随模拟器附带,要么从 Gazebo 模型库 下载。新版 Gazebo 不附带这些模型,我们可以改用 Fuel 中的模型,或者直接将模型添加到世界文件中。

要使用燃料模型,请将 标签 include 替换 sunground_place

<include>
<uri>
https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane
</uri>
</include>

<include>
<uri>
https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun
</uri>
</include>

作为参考,您可以 在此处找到 Gazebo Classic 空世界 ,并 在此处找到更新的新 Gazebo 空世界 。重新启动 empty_world.launch.py 现在应该可以成功启动模拟器。

生成

在此步骤中,我们将修改 turtlebot3_gazebo/launch/spawn_turtlebot3.launch.py 。同样,我们需要将其更改 gazebo_rosros_gz_sim 。我们还需要将其更改 spawn_entity.pycreate ,它是 中提供模型生成功能的节点 ros_gz_sim 。从参数列表中, -entity 需要将其替换为 -name 。您可以运行 以查看更多选项。 ros2 run ros_gz_sim create --helpshort

结果 Node 看起来应该是这样的:

start_gazebo_ros_spawner_cmd = Node(
package='ros_gz_sim',
executable='create',
arguments=[
'-name', TURTLEBOT3_MODEL,
'-file', urdf_path,
'-x', x_pose,
'-y', y_pose,
'-z', '0.01'
],
output='screen',
)

如果您取消注释 ld.add_action(spawn_turtlebot_cmd)empty_world.launch.py 运行启动文件,您会注意到与无法识别的插件相关的错误。这些错误来自模型 SDFormat 文件,我们接下来将修改该文件。

模型

在本教程中我们将使用 waffle 机器人,因此我们将编辑文件 turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf 。我们需要做的更改主要与插件及其参数有关。您可以 在此处参考编辑前的 Waffle 模型 SDF 文件在此处参考编辑后的 Waffle 模型 SDF 文件。以下是原始模型中所有插件的列表。对于每个插件,如果不再需要,我们将删除它,或者使用新 Gazebo 中的等效插件。您可以使用功能比较页面( FortressHarmonic )来找出 Gazebo 中是否提供 Gazebo Classic 功能(例如传感器类型)。如果使用了等效插件,我们将更新插件的 SDF 参数以匹配新插件的参数。查看系统列表( FortressHarmonic )以查找等效插件及其参数。

libgazebo_ros_imu_sensor.so

原始模型中的插件

此插件可以移除,因为有一个通用的 IMU 插件可以处理所有 IMU 传感器。我们稍后会将其添加到世界中。我们将 <topic> 其中的标签设置 <sensor> 为简短的主题名称,以便稍后创建 ROS 桥接器时使用。完整的 <sensor> 标签现在应如下所示:

<link name="imu_link">
<sensor name="tb3_imu" type="imu">
<always_on>true</always_on>
<update_rate>200</update_rate>
<topic>imu</topic>
<imu>
... <!-- all the content of <imu> -->
</imu>
</sensor>
</link>

libgazebo_ros_ray_sensor.so

原始模型中的插件

与 IMU 类似,我们将使用一个加载到世界中的通用插件来处理所有渲染传感器,其中包括激光雷达传感器。目前, ray 新版 Gazebo 不支持使用物理引擎生成传感器数据的传感器类型,我们需要将其更新为 gpu_lidar 。我们还需要将 <ray> 其中的标签更改 <sensor><lidar>frame_name 插件的参数将通过设置 gz_frame_id 中的参数来处理 <sensor> 。最后,我们将设置 <topic> 与 IMU 传感器类似的参数。激光雷达的最终 <sensor> 标签应如下所示:

<sensor name="hls_lfcd_lds" type="gpu_lidar">
<always_on>true</always_on>
<visualize>true</visualize>
<pose>-0.064 0 0.121 0 0 0</pose>
<update_rate>5</update_rate>
<topic>scan</topic>
<gz_frame_id>base_scan</gz_frame_id>
<lidar>
... <!-- same content as <ray> in the original -->
</lidar>
</sensor>

libgazebo_ros_camera.so

原始模型中的插件

相机传感器还将使用一个通用插件来处理加载到世界中的所有渲染传感器。在 SDF 文件中,我们将设置inside 和 inside <topic> 的标签 ,这两个标签稍后都会在 ROS 桥接器中使用。我们还需要设置 , 因为新版 Gazebo 中通用插件使用的默认帧 ID 与 Gazebo Classic 中使用的 不同。最终的 标签应如下所示: <sensor> <camera_info_topic> <camera> <gz_frame_id> libgazebo_ros_camera <sensor>

<sensor name="camera" type="camera">
<always_on>true</always_on>
<visualize>true</visualize>
<update_rate>30</update_rate>
<topic>camera/image_raw</topic>
<gz_frame_id>camera_rgb_frame</gz_frame_id>
<camera name="intel_realsense_r200">
<camera_info_topic>camera/camera_info</camera_info_topic>
... <!-- all the content of <camera> from the original -->
</camera>
</sensor>

libgazebo_ros_diff_drive.so

原始模型中的插件

由于这是一个特定于模型的插件,我们将用插件替换它。我们将 尽可能 DiffDrive 匹配的参数,但可能无法完全匹配。例如,原始插件有一个 ,但 有 一个,它们并不等效;后者是对整个车辆线性加速度的限制。我们可以通过将车轮加速度限制乘以车轮半径来近似该值。有关每个参数的详细信息,请参阅 DiffDrive 类参考 。以下是完整的 标签,其中包含描述原始插件映射的注释。 libgazebo_ros_diff_drive max_wheel_acceleration gz-sim-diff-drive-system max_linear_acceleration <plugin>

<plugin filename="gz-sim-diff-drive-system" name="gz::sim::systems::DiffDrive">
<!-- Remove <ros> tag. -->

<!-- wheels -->
<left_joint>wheel_left_joint</left_joint>
<right_joint>wheel_right_joint</right_joint>

<!-- kinematics -->
<wheel_separation>0.287</wheel_separation>
<wheel_radius>0.033</wheel_radius> <!-- computed from <wheel_diameter> in the original plugin-->

<!-- limits -->
<max_linear_acceleration>0.033</max_linear_acceleration> <!-- computed from <max_linear_acceleration> in the original plugin-->

<topic>cmd_vel</topic> <!-- from <commant_topic> -->

<odom_topic>odom</odom_topic> <!-- from <odometry_topic> -->
<frame_id>odom</frame_id> <!-- from <odometry_frame> -->
<child_frame_id>base_footprint</child_frame_id> <!-- from <robot_base_frame> -->
<odom_publisher_frequency>30</odom_publisher_frequency> <!-- from <update_rate>-->

<tf_topic>/tf</tf_topic> <!-- Short topic name for tf output -->

</plugin>

<wheel_torque> 参数可以通过设置每个 的努力限制来实现 <joint> 。例如:

<joint name="wheel_right_joint" type="revolute">
<parent>base_link</parent>
<child>wheel_right_link</child>
<pose>0.0 -0.144 0.023 -1.57 0 0</pose>
<axis>
<xyz>0 0 1</xyz>
<limit>
<effort>20</effort> <!-- from <wheel_torque> in libgazebo_ros_diff_drive.so.-->
</limit>
</axis>
</joint>

libgazebo_ros_joint_state_publisher.so

原始模型中的插件

我们也将用 替换此插件 JointStatePublisher 。参数大致相似,但是 <update_rate> 参数不受支持。以下是完整的 <plugin> 标签,其中包含描述原始插件映射的注释。

<plugin filename="gz-sim-joint-state-publisher-system"
name="gz::sim::systems::JointStatePublisher">
<topic>joint_states</topic> <!--from <ros><remapping> -->
<joint_name>wheel_left_joint</joint_name>
<joint_name>wheel_right_joint</joint_name>
</plugin>

世界

如前所述,传感器由通用的世界级插件处理。

默认情况下,如果没有指定世界插件,Gazebo 会添加 Physics、SceneBroadcaster 和 UserCommands 插件。但是,如果我们指定了任何插件,Gazebo 会假定我们想要覆盖默认插件,因此不会添加任何默认插件。

因此,除了默认添加的插件外,我们还必须添加 IMU 和激光雷达传感器的附加插件。我们将再次编辑 turtlebot3_gazebo/worlds/empty_world.world 以下内容,紧接着添加 。 <world name="default">

<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<plugin
filename="gz-sim-sensors-system"
name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
<plugin
filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
</plugin>

桥接 ROS

在 Gazebo Classic 中,与 ROS 的通信是通过 gazebo_ros_pkgs 直接与模拟器交互的插件实现的。相比之下,在新版 Gazebo 中,与 ROS 的通信主要通过 提供的主题桥接器进行 ros_gz 。桥接节点是一个通用节点,用于桥接 Gazebo gz-transport 和 ROS 2 之间的主题。

为了创建桥接器,我们将使用一个 yaml 包含主题名称及其映射的文件。我们将添加一个新目录 paramsturtlebot3_gazebo 创建 turtlebot3_waffle_bridge.yaml 以下内容:

# gz topic published by the simulator core
- ros_topic_name: "clock"
gz_topic_name: "clock"
ros_type_name: "rosgraph_msgs/msg/Clock"
gz_type_name: "gz.msgs.Clock"
direction: GZ_TO_ROS

# gz topic published by JointState plugin
- ros_topic_name: "joint_states"
gz_topic_name: "joint_states"
ros_type_name: "sensor_msgs/msg/JointState"
gz_type_name: "gz.msgs.Model"
direction: GZ_TO_ROS

# gz topic published by DiffDrive plugin
- ros_topic_name: "odom"
gz_topic_name: "odom"
ros_type_name: "nav_msgs/msg/Odometry"
gz_type_name: "gz.msgs.Odometry"
direction: GZ_TO_ROS

# gz topic published by DiffDrive plugin
- ros_topic_name: "tf"
gz_topic_name: "tf"
ros_type_name: "tf2_msgs/msg/TFMessage"
gz_type_name: "gz.msgs.Pose_V"
direction: GZ_TO_ROS

# gz topic subscribed to by DiffDrive plugin
- ros_topic_name: "cmd_vel"
gz_topic_name: "cmd_vel"
ros_type_name: "geometry_msgs/msg/Twist"
gz_type_name: "gz.msgs.Twist"
direction: ROS_TO_GZ

# gz topic published by IMU plugin
- ros_topic_name: "imu"
gz_topic_name: "imu"
ros_type_name: "sensor_msgs/msg/Imu"
gz_type_name: "gz.msgs.IMU"
direction: GZ_TO_ROS

# gz topic published by Sensors plugin
- ros_topic_name: "scan"
gz_topic_name: "scan"
ros_type_name: "sensor_msgs/msg/LaserScan"
gz_type_name: "gz.msgs.LaserScan"
direction: GZ_TO_ROS

# gz topic published by Sensors plugin (Camera)
- ros_topic_name: "camera/camera_info"
gz_topic_name: "camera/camera_info"
ros_type_name: "sensor_msgs/msg/CameraInfo"
gz_type_name: "gz.msgs.CameraInfo"
direction: GZ_TO_ROS

完整的 yaml 文件可以在这里找到。yaml 文件中的每个条目都包含一个 ROS 主题名称、一个 Gazebo 主题名称、一个 ROS 数据/消息类型以及一个指示消息流向的方向。我们需要更新该 CMakeLists.txt 文件以安装我们创建的新 params 目录。CMake install 命令如下所示:

最后,我们将编辑 turtlebot3_gazebo/launch/spawn_turtlebot3.launch.py ,以创建桥节点:

您可能已经注意到,在桥接参数中,我们没有包含 camera/image_raw 主题。虽然可以像桥接其他所有主题一样桥接图像主题,但我们将使用专门的桥接节点, ros_gz_image 它为图像主题提供了更高效的桥接。我们将以下代码片段添加到 turtlebot3_gazebo/launch/spawn_turtlebot3.launch.py

start_gazebo_ros_image_bridge_cmd = Node(
package='ros_gz_image',
executable='image_bridge',
arguments=['/camera/image_raw'],
output='screen',
)

最后,我们将所有新操作添加到 函数 LaunchDescription 返回的列表中 generate_launch_description

# ...

# Add the action to \`ld\` toward the end of the file

ld.add_action(start_gazebo_ros_bridge_cmd)
ld.add_action(start_gazebo_ros_image_bridge_cmd)

您可以在此处 找到 Gazebo Classic spawn_turtlebot3.launch.py 文件,并 在此处 找到更新的 Gazebo spawn_turtlebot3.launch.py 文件。

现在我们准备启动空的世界,它会生成华夫饼机器人并建立桥梁,以便我们可以从 ROS 2 与它进行通信。

export TURTLEBOT3_MODEL=waffle
ros2 launch turtlebot3_gazebo empty_world.launch.py

这是启动 Turtlebot3 在 Gazebo 中运行的截图 empty_world.launch.py 。通过添加“Visualize Lidar” GUI 插件可以启用激光雷达可视化功能(请参阅 如何添加 GUI 插件的 教程)。

Turtlebot 3 在 Gazebo 中运行的截图

现在也可以 从 Turtlebot3 手册中学习 SLAM导航 turtlebot3_world.world 教程(请确保选择“Humble”选项卡)。但是,这需要根据我们在本教程中讨论的内容更新相关 文件。作为参考,这些文件也已在 此 fork turtlebot3_world.launch.py 中迁移 。

迁移 turtlebot3_gazebo 中的其他

本教程并未涵盖从 Gazebo Classic 迁移模型和启动文件的所有方面。请参阅 Gazebo Classic 迁移 文档,获取更多有助于迁移其他方面的资源,例如 Gazebo Classic 插件、材质和纹理。